#! /usr/bin/python

# +-----------------------------+
# |        超声波避障
# | - 引脚说明：
# |
# | - 功能说明：
# | 每一秒返回一个距离
# +-----------------------------+

# ! 超声波测距原理
# 超声波一共有四个引脚，分别控制Trig Echo VCC GND
# VCC GND 分别连接电源正极和地线。
# 当需要测距时， 我们控制Trig引脚，产生一个至少10us的高电平脉冲信号（相当于发出声波，遇到障碍物会反射回来）
# 然后监控Echo引脚的返回信号，Echo引脚原本是低电平，它会先拉高再拉低，其中 高电平持续时间 就是传感器从发出信号到接收到信号的时间。
# 根据声速，可以测量出障碍物到超声波的距离。
# 一般使用的超声波模块的测量范围为 2mm~2000mm

# Coded by Qi

import RPi.GPIO as GPIO
from time import time, sleep

class Ultrasound:
    def __init__(self, trig, echo) -> None:
        self.trig = trig
        self.echo = echo
        # 指定GPIO的编码方式, 这里使用BCM编码方式。
        # 具体信息参考 https://pinout.xyz/pinout/5v_power
        GPIO.setmode(GPIO.BCM)

        # (非必需)关闭警告信息
        GPIO.setwarnings(False)

        # 信号发送端 设置为输出
        GPIO.setup(trig, GPIO.OUT, initial=GPIO.LOW)
        # 信号接收端 设置为输入
        GPIO.setup(echo, GPIO.IN)

    def __delay_us(self, microsecond):
        '''
        延时函数 单位：微秒(us)
        '''
        sleep(microsecond / 1000000)


    def get_distance(self):
        '''
        获得障碍物到传感器的距离。
        主要分为三步：发射信号 接收信号 计算距离。
        '''
        # 第一步 发射测距信号
        GPIO.output(self.trig, GPIO.LOW)
        self.__delay_us(2)  # 延时2us
        GPIO.output(self.trig, GPIO.HIGH)
        self.__delay_us(10)  # 延时10us
        GPIO.output(self.trig, GPIO.LOW)

        # 第二步 接收脉冲信号
        start_time = 0  # 记录低电平结束时间
        end_time = 0 # 记录高电平结束时间
        while GPIO.input(self.echo) == GPIO.LOW:
            start_time = time()
        while GPIO.input(self.echo) == GPIO.HIGH:
            end_time = time()
        pulse_time = end_time - start_time  # 脉冲时间

        # 第三步 计算测量距离
        # 公式：(pulse_time * 34300(声速34300cm/s)) / 2 (往返) (单位：cm)
        return (pulse_time * 34300) / 2


if __name__ == '__main__':
    echo = 9
    trig = 10
    us = Ultrasound(trig, echo)
    while True:
        print(f"Distance now is {us.get_distance()}cm")
        sleep(1)
